from .task_parser import *
from algorithm.tsp import TSP

# 直接运行 move_tools.py 用这个引用
# from wcs_logic_tools import *
# from tsp_forTest import *

######### 移库工具模块 #########

class TasksProcessor:
    def __init__(self):
        self.task_parser = TaskParser()
        self.tsp = TSP()
    
    def read_tasks_target_list(self, tasks):
        """
        读取任务所有节点
        """
        point_list = []
        for task in tasks:
            Description = task["Description"]
            for desc in Description:
                target = desc["target"]
                point_list.append(target)
        return point_list


    def find_hinder_point(self, data, tasks):
        """
        找到任务中被遮挡的节点列表
        """

        # 加载任务节点
        target_list = self.read_tasks_target_list(tasks)
        # print(f"所有任务节点: {target_list}")

        # 找到遮挡节点,并生成遮挡节点列表
        hinder_point = []
        # 任务库位的状态
        tasks_point_status = []
        # 遮挡库位状态表
        hinder_point_status = []

        for task in tasks:
            all_col = data["warehouseStatus"]
            if self.task_parser.get_task_warehouseStatus(task) == 0:
                # 任务排为0
                tasks_col_num = 0
                tasks_col = all_col[tasks_col_num]
                
                # 遮挡排为1
                hinder_col_num = 1
                hinder_col = all_col[hinder_col_num]
                
                tasks_Description = task["Description"]
                # 取得任务每个节点的库位状态
                for desc in tasks_Description:
                    tasks_WHstatus = desc["target"]
                    tasks_point_status.append(tasks_col[tasks_WHstatus])

                # 取得任务前排节点的库位状态
                for desc in tasks_Description:
                    tasks_WHstatus = desc["target"]
                    hinder_point_status.append(hinder_col[tasks_WHstatus])

            elif self.task_parser.get_task_warehouseStatus(task) == 3:
                # 任务排为3
                tasks_col_num = 3
                tasks_col = all_col[tasks_col_num]
                
                # 遮挡排为2
                hinder_col_num = 2
                hinder_col = all_col[hinder_col_num]

                tasks_Description = task["Description"]
                # 取得任务每个节点的库位状态
                for desc in tasks_Description:
                    tasks_WHstatus = desc["target"]
                    tasks_point_status.append(tasks_col[tasks_WHstatus])

                # 取得任务前排节点的库位状态
                for desc in tasks_Description:
                    tasks_WHstatus = desc["target"]
                    hinder_point_status.append(hinder_col[tasks_WHstatus])

        # print(f"所有任务对应库位状态: {tasks_point_status}")
        # print(f"所有任务库位的前排库位状态: {hinder_point_status}")

        # 找出状态相同的库位
        same_status_positions = []
        for i in range(len(target_list)):
            if tasks_point_status[i] == hinder_point_status[i]:
                same_status_positions.append(target_list[i])

        # 输出除去状态相同库位后的仓库库位号码列表
        remaining_positions = [pos for pos in target_list if pos not in same_status_positions]

        # print(f"被遮挡的库位: {remaining_positions}")
        # print(f"没被遮挡的库位: {same_status_positions}")

        return same_status_positions, remaining_positions

    
    def split_tasks(self, tasks):
        """
        拆分前后排任务
        """
        
        # 前排
        front_1_tasks_list = []
        front_2_tasks_list = []

        # 分割前后排的节点
        for task in tasks:
            if task['Description'][0]['warehouseStatus'] == 1:
                front_1_tasks_list.append(task)  # 保存整个任务对象
            elif task['Description'][0]['warehouseStatus'] == 2:
                front_2_tasks_list.append(task)  # 保存整个任务对象
        # print(front_1_tasks_list)
        # print(front_2_tasks_list)
        
        # 重组前排任务
        # front_1_tasks: 根据 target 排序
        front_1_tasks = sorted(front_1_tasks_list, key=lambda x: x['Description'][0]['target'])
        
        # front_2_tasks: 根据 target 排序
        front_2_tasks = sorted(front_2_tasks_list, key=lambda x: x['Description'][0]['target'])
            
        return front_1_tasks, front_2_tasks


    def split_hind_tasks(self, back_tasks, data):
        """
        拆分被遮挡和无遮挡任务
        """
        
        # 先找到遮挡的任务点
        no_hinder_list = self.find_hinder_point(data, back_tasks)[0]
        is_hinder_list = self.find_hinder_point(data, back_tasks)[1]
        
        # 重组前排任务
        # 根据 target 重新找到完整的任务对象
        no_hinder_tasks = [task for task in back_tasks if task['Description'][0]['target'] in no_hinder_list]
        is_hinder_tasks = [task for task in back_tasks if task['Description'][0]['target'] in is_hinder_list]
            
        return no_hinder_list, no_hinder_tasks, is_hinder_list, is_hinder_tasks



    def find_front_free_point(self, data, tasks, front_col_id):

        """
        找到前排所有空库

        找到 status_list 中值为 0 且索引不在 target_list 中的索引。
        
        参数:
        - target_list: 要排除的索引列表。
        - status_list: 状态列表。

        返回:
        - free_indices: 值为 0 且不在 point_list 中的索引列表。
        """
        # 加载任务节点
        target_list = self.read_tasks_target_list(tasks)

        # 加载前排所有节点状态
        status_list = data["warehouseStatus"][front_col_id]

        # 找到 status_list 中值为 0 且索引不在 point_list 中的索引
        free_indices = [i for i, status in enumerate(status_list) if status == 0 and i not in target_list]
        
        return free_indices


    def take_path(self, point_a, point_b):
        """
        取货路径
        """
        path = [1, point_a, point_b]
        return path

    def putdown_path(self, point_a, point_b):
        """
        放货路径
        """
        path = [2, point_a, point_b]
        return path

    
    def process_front(self, front_tasks, plc_pos, warehouse_Status):
        """
        前排任务生成路径
        处理前排只要考虑直接生成路径即可，并修改好对应的库位状态
        """
        resp_list = []
        # 逐个读取任务
        for task in front_tasks:
            task_no = task['No']

            # 任务所在排
            task_whs = self.task_parser.get_task_warehouseStatus(task)

            # 任务类型
            task_type = self.task_parser.get_task_type(task)
            # print(f'任务类型：{task_type}')

            # 入库
            if task_type == 0:
                
                # 起点位置 = 读取设备点(堆垛机位置)
                start_pos = plc_pos
                
                # 目标位置 = 读取任务点(目标库位位置)
                target_pos = self.task_parser.get_task_target_point(task)
                # responses.append(target_pos)
                
                # 入库口位置 = 入库口
                in_pos = self.task_parser.get_task_inpoint(task)
                
                # 子路径, 堆垛机位置 = 生成路径(起点位置，目标位置)
                # start2target_path = dijs.run(start_pos, target_pos)
                # 取货
                start2in_path = self.take_path(start_pos, in_pos)
                
                # 子路径, 堆垛机位置 = 生成路径(堆垛机位置，出库口位置)
                # in2target_path = dijs.run(target_pos, out_pos)
                # 放货
                in2target_path = self.putdown_path(in_pos, target_pos)

                # 修改库位状态(空库0, 有货2)
                self.task_parser.switch_position_status(warehouse_Status, wh_col=task_whs-1, wh_posi=target_pos, new_point=2)
                
                # 放入json数据
                resp_list.append({
                                    "no": task_no,
                                    "Type": self.task_parser.get_task_type(task),
                                    "warehouseStatus": task_whs,
                                    "path_1": start2in_path,
                                    "path_2": in2target_path,
                                    "endStackerPosition": target_pos,
                                })
                # print(f'{start2in_path},{in2target_path}')
                
                # 堆垛机最终位置 = 出库口位置
                plc_pos = target_pos
            
            # 出库
            elif task_type == 1:
                
                # 起点位置 = 读取设备点(堆垛机位置)
                start_pos = plc_pos
                
                # 目标位置 = 读取任务点(目标库位位置)
                target_pos = self.task_parser.get_task_target_point(task)
                # responses.append(target_pos)

                # 出库口位置 = 出库口
                out_pos = self.task_parser.get_task_outpoint(task)
                
                # 子路径, 堆垛机位置 = 生成路径(起点位置，目标位置)
                # start2target_path = dijs.run(start_pos, target_pos)
                # 取货
                start2target_path = self.take_path(start_pos, target_pos)
            
                # 修改库位状态(有货2, 空库0)
                self.task_parser.switch_position_status(warehouse_Status, wh_col=task_whs-1, wh_posi=target_pos, new_point=0)
                
                # 子路径, 堆垛机位置 = 生成路径(堆垛机位置，出库口位置)
                # target2out_path = dijs.run(target_pos, out_pos)
                # 放货
                target2out_path = self.putdown_path(target_pos, out_pos)
                    
                # 放入json数据
                resp_list.append({
                                    "no": task_no,
                                    "Type": self.task_parser.get_task_type(task),
                                    "warehouseStatus": task_whs,
                                    "path_1": start2target_path,
                                    "path_2": target2out_path,
                                    "endStackerPosition": out_pos,
                                })
                # print(f'{start2target_path},{target2out_path}')
                    
                # 堆垛机最终位置 = 出库口位置
                plc_pos = out_pos

            else:
                print("ERROR!")
        
        return plc_pos, resp_list

    # 后排任务生成路径
    # 处理后排需要先判断是否有遮挡，有遮挡则先将遮挡任务拆分成：无遮挡、有遮挡
    # 然后调用前面无遮挡方法处理无遮挡，最后再处理有遮挡的任务
    def process_back(self, back_tasks, front_free_list, plc_pos, io_pos, distance_matrix, warehouse_Status):
        resp_list = []
        # 逐个读取任务
        for task in back_tasks:
            task_no = task['No']
            
            # 任务所在排
            task_whs = self.task_parser.get_task_warehouseStatus(task)

            # 任务类型
            task_type = self.task_parser.get_task_type(task)
            # print(f'任务类型：{task_type}')

            # 入库
            if task_type == 0:
                
                # 起点位置 = 读取设备点(堆垛机位置)
                start_pos = plc_pos
                
                # 目标位置 = 读取任务点(目标库位位置)
                target_pos = self.task_parser.get_task_target_point(task)
                # responses.append(target_pos)
                
                # 入库口位置 = 入库口
                in_pos = io_pos
                
                #### 移走障碍物 ####
                # 起点位置 -> 目标位置 (取货)
                start2target_path = self.take_path(start_pos, target_pos)
                # 目标位置 -> 最近空库位置 (放货)
                target2free_path = self.tsp.near_point(target_pos, front_free_list)
                target2free_path = self.putdown_path(target2free_path[0], target2free_path[1])
                # 记录空库点
                near_free_point = target2free_path[2]
                
                #### 取货入库 ####
                # 最近空库位置 -> 入库口位置 (取货)
                free2in_path = self.take_path(near_free_point, in_pos)
                # 入库口位置 -> 目标位置 (放货)
                in2target_path = self.putdown_path(in_pos, target_pos)
                # 修改库位状态(空库0, 有货2)
                self.task_parser.switch_position_status(warehouse_Status, wh_col=task_whs, wh_posi=target_pos, new_point=2)
                
                #### 移走的障碍物回到原位 ####
                # 目标位置 -> 记录空库点 (取货)
                target2free_path = self.take_path(target_pos, near_free_point)
                # 记录空库点 -> 目标位置 (放货)
                free2target_path = self.putdown_path(near_free_point, target_pos)

                # 放入json数据
                resp_list.append({
                                    "no": task_no,
                                    "Type": self.task_parser.get_task_type(task),
                                    "warehouseStatus": task_whs,
                                    "path_1": start2target_path,
                                    "path_2": target2free_path,
                                    "path_3": free2in_path,
                                    "path_4": in2target_path,
                                    "path_5": target2free_path,
                                    "path_6": free2target_path,
                                    "endStackerPosition": target_pos,
                                })
                # print(f'{start2in_path},{in2target_path}')
                
                # 堆垛机最终位置 = 出库口位置
                plc_pos = target_pos
            
            # 出库
            elif task_type == 1:
                
                # 起点位置 = 读取设备点(堆垛机位置)
                start_pos = plc_pos
                
                # 目标位置 = 读取任务点(目标库位位置)
                target_pos = self.task_parser.get_task_target_point(task)
                # responses.append(target_pos)

                # 出库口位置 = 出库口
                out_pos = io_pos
                
                #### 移走障碍物 ####
                # 起点位置 -> 目标位置 (取货)
                start2target_path = self.take_path(start_pos, target_pos)
                # 目标位置 -> 最近空库位置 (放货)
                target2free_path = self.tsp.near_point(target_pos, front_free_list)
                target2free_path = self.putdown_path(target2free_path[0], target2free_path[1])
                # 记录空库点
                near_free_point = target2free_path[2]

                #### 取货出库 ####
                # 最近空库位置 -> 目标位置 (取货)
                free2in_path = self.take_path(near_free_point, target_pos)
                # 修改库位状态(有货2, 空库0)
                self.task_parser.switch_position_status(warehouse_Status, wh_col=task_whs, wh_posi=target_pos, new_point=0)
                # 目标位置 -> 出库口位置 (放货)
                target2out_path = self.putdown_path(target_pos, out_pos)
                
                #### 移走的障碍物回到原位 ####
                # 出库口位置 -> 记录空库点 (取货)
                out2free_path = self.take_path(out_pos, near_free_point)
                # 记录空库点 -> 目标位置 (放货)
                free2target_path = self.putdown_path(near_free_point, target_pos)
                    
                # 放入json数据
                resp_list.append({
                                    "no": task_no,
                                    "Type": self.task_parser.get_task_type(task),
                                    "warehouseStatus": task_whs,
                                    "path_1": start2target_path,
                                    "path_2": target2free_path,
                                    "path_3": free2in_path,
                                    "path_4": target2out_path,
                                    "path_5": out2free_path,
                                    "path_6": free2target_path,
                                    "endStackerPosition": out_pos,
                                })
                # print(f'{start2target_path},{target2out_path}')
                    
                # 堆垛机最终位置 = 出库口位置
                plc_pos = out_pos

            else:
                print("ERROR!")

        return plc_pos, resp_list

############### test code ###########################
# data = read_json()
# tasks = data["Tasks"]

# print(f"无遮挡的库位: {find_hinder_point(data, tasks)[0]}")
# print(f"被遮挡的库位: {find_hinder_point(data, tasks)[1]}")

# no_hinder_list = find_hinder_point(data, tasks)[0]
# is_hinder_list = find_hinder_point(data, tasks)[1]

# 找到前排空闲节点
# free_front_positions = find_front_free_point(data, tasks, 2)
# print(free_front_positions)